#include "radar_protocol.h"
#include "log.h"
#include "event.h"
#include "fusion.h"
#include "platform.h"

extern radar_manager_t g_radar_manager;

bool check_radar_packet(uint8_t *buffer, int length, radar_packet_t *radar_packet_packet)
{
    uint32_t head = *(uint32_t *)buffer;
    uint16_t data_length = *(uint16_t *)(buffer + 4);

    if (head != RADAR_PACKET_HEADER) {
        // printf("head error: %8X\n", head);
        return false;
    }
    uint32_t tail = *(uint32_t *)(buffer + length - 4);
    if (tail != RADAR_PACKET_TAIL) {
        // printf("tail error: %8X\n", tail);
        return false;
    }
    uint32_t checksum = *(uint32_t *)(buffer + length - 2 * 4);
    uint32_t crc = pro_crc32_mpeg((uint8_t *)(buffer + 4), length - 3 * 4);
    if (crc != checksum) {
        // printf("crc: %8X, checksum: %8X\n", crc, checksum);
        return false;
    }
    if (data_length != length) {
        // printf("data_length: %d, length: %d\n", data_length, length);
        return false;
    }
    memcpy(radar_packet_packet, buffer, length - 4 - 4);
    radar_packet_packet->checksum = checksum;
    radar_packet_packet->tail = tail;
    return true;
}

static void analysis_65_packet(char *buffer, uint8_t dev_type)
{
    radar_packet_t *packet = (radar_packet_t *)buffer;
    radar_packet_65_t *radar_packet_65 = (radar_packet_65_t *)packet->data;
    data_65_header_t *header = &radar_packet_65->header;
    uint32_t radar_frame_num = (header->radar_frame_num[0] | header->radar_frame_num[1] << 8 | header->radar_frame_num[2] << 16);
    uint16_t obj_num = header->obj_num;
    int i, j;
    bool find = false;
    char buf[256] = {0};

    if (obj_num > 0) {
        write_log("测试接力", "=============================================", NULL);
    }

    if (g_config.radar_debug == 1) {
        printf("---------------------------------------------\n");
    }

    //  printf("obj_num: %d\n", header->obj_num);
    // if (obj_num > 0) {
    radar_frame_t radar_frame = {0};
    data_65_body_t *body = radar_packet_65->body;
    Bitmap obj_map = {0};
    for (int i = 0; i < obj_num; i++) {
        data_65_body_t *obj = &body[i];
        radar_obj_info_t radar_obj_info = {0};
        radar_obj_info.id = obj->obj_id;
        radar_obj_info.lane = lane_to_index(obj->obj_lane);
        radar_obj_info.type = obj->obj_type;
        radar_obj_info.length = obj->obj_length;
        radar_obj_info.dist_y = obj->obj_dist_y_low;
        radar_obj_info.dist_x = obj->obj_dist_x_low;
        radar_obj_info.speed_y = obj->obj_speed_y;
        radar_obj_info.speed_x = obj->obj_speed_x;
        radar_obj_info.speed = obj->obj_speed;
        radar_obj_info.longitude = (double)obj->obj_longitude / 1e7;
        radar_obj_info.latitude = (double)obj->obj_latitude / 1e7;
        radar_obj_info.height = obj->obj_height;

        if (radar_frame.obj_num >= RADAR_INFO_SIZE) {
            break;
        }
        if (radar_obj_info.lane == -1) {
            continue;
        }

        update_new_enter_obj(&radar_obj_info, dev_type);
        if (filter_radar_obj(&radar_obj_info, dev_type) == true) {
            continue;
        }

        // if (is_false_target(radar_obj_info.id) == true) {
        //     continue;
        // }

        // if (dev_type == RADAR_INCOMING && radar_obj_info.lane != -1 && radar_obj_info.dist_y < g_config.in_radar_max_distance) {
        if (dev_type == RADAR_INCOMING && radar_obj_info.lane != -1) {
            // if (dev_type == RADAR_INCOMING) {
            // sprintf(buf, ">>>>>>>>>>>>>>>>>来向雷达ID: 0x%04X, 道路号: %d, 距离: %d, 速度: %d", radar_obj_info.id, obj->obj_lane, radar_obj_info.dist_y, radar_obj_info.speed_y);
            sprintf(buf, ">>>>>>>>>>>>>>>>>来向雷达ID: 0x%04X, 道路号: %d, 距离y: %d, 距离x: %d, 速度: %d,经度: %f, 纬度: %f, 高度: %d", radar_obj_info.id, radar_obj_info.lane, radar_obj_info.dist_y, radar_obj_info.dist_x, radar_obj_info.speed_y, radar_obj_info.longitude, radar_obj_info.latitude, radar_obj_info.height);
            if (g_config.radar_debug == 1) {
                printf("\033[32m%s\033[0m\n", buf);
            }
            write_log("测试接力", buf, NULL);
        }

        // if (dev_type == RADAR_OUTGOING && radar_obj_info.lane != -1 && radar_obj_info.dist_y < OUT_RADAR_VIEW_MAX_DISTANCE) {
        if (dev_type == RADAR_OUTGOING && radar_obj_info.lane != -1) {
            // if (dev_type == RADAR_OUTGOING) {
            // sprintf(buf, "<<<<<<<<<<<<<<<<<去向雷达ID: 0x%04X, 道路号: %d, 距离: %d, 速度: %d", radar_obj_info.id, radar_obj_info.lane, radar_obj_info.dist_y, radar_obj_info.speed_y);
            sprintf(buf, "<<<<<<<<<<<<<<<<<去向雷达ID: 0x%04X, 道路号: %d, 距离y: %d, 距离x: %d, 速度: %d,经度: %f, 纬度: %f, 高度: %d", radar_obj_info.id, radar_obj_info.lane, radar_obj_info.dist_y, radar_obj_info.dist_x, radar_obj_info.speed_y, radar_obj_info.longitude, radar_obj_info.latitude, radar_obj_info.height);
            if (g_config.radar_debug == 1) {
                printf("\033[34m%s\033[0m\n", buf);
            }
            write_log("测试接力", buf, NULL);
        }
        update_obj_statistics(&radar_obj_info, dev_type);

        if (dev_type == RADAR_INCOMING) {
            if (radar_obj_info.dist_y < g_config.in_radar_max_distance && radar_obj_info.dist_y >= g_config.in_radar_min_distance + g_config.in_radar_offset) {
                int bit_y = MAX_COLS * (radar_obj_info.dist_y - g_config.in_radar_min_distance) / (g_config.in_radar_max_distance - g_config.in_radar_min_distance);
                set_bit(&obj_map, radar_obj_info.lane, bit_y);
                radar_obj_info.row = radar_obj_info.lane;
                radar_obj_info.col = bit_y;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
                // printf("\033[32mlane: %d, dist_y: %d\033[0m\n", radar_obj_info.lane, radar_obj_info.dist_y);
            }
            else if (radar_obj_info.dist_y > 0) {
                radar_obj_info.row = -1;
                radar_obj_info.col = -1;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
                // printf("\033[31mlane: %d, dist_y: %d\033[0m\n", radar_obj_info.lane, radar_obj_info.dist_y);
            }
            if (radar_obj_info.dist_y < 100 && g_config.need_fusion == 1) {
                clac_coordinate_speed(&radar_obj_info, g_radar_manager.in_radar_frame_queue);
                append_relays_queue(&radar_obj_info);
                continue;
            }
        }
        else {
            if (radar_obj_info.dist_y < g_config.out_radar_max_distance && radar_obj_info.dist_y >= g_config.out_radar_min_distance + g_config.out_radar_offset) {
                int bit_y = MAX_COLS * (radar_obj_info.dist_y - g_config.out_radar_min_distance) / (g_config.out_radar_max_distance - g_config.out_radar_min_distance);
                set_bit(&obj_map, radar_obj_info.lane, bit_y);
                radar_obj_info.row = radar_obj_info.lane;
                radar_obj_info.col = bit_y;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
            }
            else {
                radar_obj_info.row = -1;
                radar_obj_info.col = -1;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
            }
        }
    }

    radar_frame.frame_time = get_current_time_in_milliseconds();
    memcpy(radar_frame.time_stamp, packet->timestamp, sizeof(radar_frame.time_stamp));
    memcpy(&radar_frame.obj_map, &obj_map, sizeof(Bitmap));

    // printf("雷达位图\n");
    // print_bitmap(&obj_map);

    if (dev_type == RADAR_INCOMING) {
        try_to_pole_relay(&radar_frame);
        queue_enqueue(g_radar_manager.in_radar_frame_queue, &radar_frame);
    }
    else {
        // if (g_config.need_fusion == 1) {
        //     try_to_relay(&radar_frame);
        // }
        try_to_pole_relay(&radar_frame);
        queue_enqueue(g_radar_manager.out_radar_frame_queue, &radar_frame);
    }

    // process_manager_overlaps(&g_radar_manager);

    if (obj_num > 0) {
        write_log("测试接力", "=============================================\n", NULL);
    }
    if (g_config.radar_debug == 1) {
        printf("---------------------------------------------\n\n");
    }
}

static void analysis_80_packet(char *buffer, uint8_t dev_type)
{
    radar_packet_t *packet = (radar_packet_t *)buffer;
    radar_packet_80_t *radar_packet_80 = (radar_packet_80_t *)packet->data;
    data_80_header_t *header = &radar_packet_80->header;
    uint8_t lane_count = header->lane_count;
    data_80_body_t *body = radar_packet_80->body;
    // char timeinfo[64] = {0};
    // printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
    // printf("%s statistic_80 cycle: %d\n", get_current_time_string(timeinfo, sizeof(timeinfo)), header->flow_cycle);
    // for (int i = 0; i < lane_count; i++) {
    //     data_80_body_t *lane_info = &body[i];
    //     printf("way_id: %d\n", lane_info->way_id);
    //     printf("a_flow_rate_big: %d\n", lane_info->a_flow_rate_big);
    //     printf("b_flow_rate_mid: %d\n", lane_info->b_flow_rate_mid);
    //     printf("c_flow_rate_small: %d\n", lane_info->c_flow_rate_small);
    //     printf("avg_time_use_rate: %d\n", lane_info->avg_time_use_rate);
    //     printf("avg_flow_rate_vehicle: %d\n", lane_info->avg_flow_rate_vehicle);
    //     printf("avg_length_vehicle: %d\n", lane_info->avg_length_vehicle);
    //     printf("avg_dis_time: %d\n", lane_info->avg_dis_time);
    //     printf("avg_dis_inter: %d\n", lane_info->avg_dis_inter);
    //     printf("bit_rate: %d\n", lane_info->statistic_85_Speed);
    //     printf("person_flow_rate: %d\n", lane_info->person_flow_rate);
    //     printf("flow_nonmotor: %d\n", lane_info->flow_nonmotor);
    // }
    // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
    send_statistics_data(radar_packet_80, dev_type);
}

bool update_event_queue_blg(uint16_t id, uint16_t event_id, uint8_t dev_type)
{
    int64_t current_time = get_current_time_in_milliseconds();
    int64_t min_time_interval = 60 * 1000;
    int16_t count = get_queue_count(g_radar_manager.event_static_queue);
    for (int i = 0; i < count; i++) {
        blg_event_static_t *event_static = (blg_event_static_t *)queue_get_node(g_radar_manager.event_static_queue, from_tail, i);
        if (event_static && event_static->event_id == event_id && event_static->dev_type == dev_type && event_static->id == id) {
            if (min_time_interval > (current_time - event_static->time_stamp)) {
                if (g_config.event_debug == 1) {
                    printf("\033[0;31m已存在相同事件: 0x%02X, %04X, %d,时间相差: %d秒\033[0m\n", event_static->event_id, event_static->id, event_static->dev_type, (int)((current_time - event_static->time_stamp) / 1000));
                }
                return false;
            }
        }
    }

    blg_event_static_t event_static = {0};
    event_static.id = id;
    event_static.event_id = event_id;
    event_static.time_stamp = current_time;
    event_static.dev_type = dev_type;
    queue_enqueue(g_radar_manager.event_static_queue, &event_static);
    return true;  // 新事件
}

void print_event_blg(radar_packet_84_t *radar_packet_84)
{
    char timeinfo[64] = {0};
    printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
    printf("%s \n", get_current_time_string(timeinfo, sizeof(timeinfo)));
    printf("event_type: 0x%02X\n", radar_packet_84->header.event_type);
    if (radar_packet_84->header.event_type == 0x31 || radar_packet_84->header.event_type == 0x32 || radar_packet_84->header.event_type == 0x41 || radar_packet_84->header.event_type == 0x42) {
        printf("test_num: %d\n", radar_packet_84->body.qt.test_num);
    }
    else {
        printf("obj_id: 0x%04X\n", radar_packet_84->body.va.obj_id);
        printf("lane_id: %d\n", radar_packet_84->body.va.obj_lane);
        printf("obj_type: %d\n", radar_packet_84->body.va.obj_type);
        printf("obj_length: %d\n", radar_packet_84->body.va.obj_length);
        printf("obj_dist_y: %d\n", radar_packet_84->body.va.obj_dist_y_low);
        printf("obj_dist_x: %d\n", radar_packet_84->body.va.obj_dist_x_low);
        printf("obj_speed_y: %d\n", radar_packet_84->body.va.obj_speed);
    }
    printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
}

static void analysis_84_packet(char *buffer, uint8_t dev_type)
{
    radar_packet_t *packet = (radar_packet_t *)buffer;
    radar_packet_84_t *radar_packet_84 = (radar_packet_84_t *)packet->data;

    if (radar_packet_84->header.event_type == 0x1 ||
        radar_packet_84->header.event_type == 0x2 ||
        radar_packet_84->header.event_type == 0x3 ||
        radar_packet_84->header.event_type == 0x4 ||
        radar_packet_84->header.event_type == 0x5 ||
        radar_packet_84->header.event_type == 0x6 ||
        radar_packet_84->header.event_type == 0x7 ||
        radar_packet_84->header.event_type == 0x8 ||
        radar_packet_84->header.event_type == 0x11 ||
        radar_packet_84->header.event_type == 0x12 ||
        radar_packet_84->header.event_type == 0x13 ||
        radar_packet_84->header.event_type == 0x14 ||
        radar_packet_84->header.event_type == 0x15 ||
        radar_packet_84->header.event_type == 0x21) {
        if (event_lane_filter(radar_packet_84->body.va.obj_lane) == false) {
            return;  // 道路号过滤
        }

        // if (g_config.event_range > 0) {
        //     if (radar_packet_84->body.va.obj_dist_y_low > g_config.event_range) {
        //         if (g_config.event_debug == 1) {
        //             printf("\033[0;31m距离超过事件检测范围: %d, ID: 0x%04X, 道路号: %d, 距离: %d, 事件类型: 0x%02X\033[0m\n",
        //                    g_config.event_range, radar_packet_84->body.va.obj_id, radar_packet_84->body.va.obj_lane, radar_packet_84->body.va.obj_dist_y_low, radar_packet_84->header.event_type);
        //         }
        //         return;  // 距离过滤
        //     }
        // }

        // if (radar_packet_84->header.event_type == 0x11) {
        //     if (dev_type == RADAR_INCOMING && radar_packet_84->body.va.obj_dist_y_low < g_config.in_radar_max_distance && video_detect_emergency_lane() == false) {
        //         if (g_config.event_debug == 1) {
        //             printf("\033[0;31m应急车道事件和视频检测冲突, ID: 0x%04X, 道路号: %d, 距离: %d\033[0m\n",
        //                    radar_packet_84->body.va.obj_id, radar_packet_84->body.va.obj_lane, radar_packet_84->body.va.obj_dist_y_low);
        //         }
        //         return;
        //     }
        // }
    }

    if (radar_packet_84->header.event_type == 0x31 ||
        radar_packet_84->header.event_type == 0x32 ||
        radar_packet_84->header.event_type == 0x41 ||
        radar_packet_84->header.event_type == 0x42) {
        if (event_lane_filter(radar_packet_84->body.qt.test_num) == false) {
            return;  // 道路号过滤
        }
    }

    if (update_event_queue_blg(radar_packet_84->body.va.obj_id, radar_packet_84->header.event_type, dev_type) == false) {
        return;  // 已存在相同事件
    }

    if (radar_packet_84->header.event_type == 0x04 || radar_packet_84->header.event_type == 0x13) {
        process_lane_change(radar_packet_84->body.va.obj_dist_y_low, dev_type);
    }
    if (g_config.event_debug == 1) {
        print_event_blg(radar_packet_84);
    }
    send_event_to_platform(radar_packet_84, packet->timestamp, dev_type);
}

void analysis_radar_buffer_blg(char *buffer, int length, uint8_t dev_type)
{
    radar_packet_t packet;

    if (g_config.radar_debug == 1) {
        // write_hex_log(dev_type == RADAR_INCOMING ? IN_RADAR_LOG : OUT_RADAR_LOG, dev_type == RADAR_INCOMING ? "来向雷达数据包" : "去向雷达数据包", buffer, length);
    }

    if (check_radar_packet(buffer, length, &packet)) {
        switch (packet.type) {
            case 0x65:
                analysis_65_packet((char *)&packet, dev_type);
                break;
            case 0x80:
                analysis_80_packet((char *)&packet, dev_type);
                break;
            case 0x84:
                analysis_84_packet((char *)&packet, dev_type);
                break;
            default:
                break;
        }
    }
}